#这用于判断两空间线段干涉情况或最近距离的类
import math
import numpy as np
import random 
import time
from decimal import *
from itertools import permutations

class TaskPipeRouting:
    def __init__(self):
        self.task_num =None
        self.group_manage = []
    def settask(self,jsondata):
        self.task_num = jsondata['task_num']
        for i in jsondata['group_manage']:
            g=Group_list(i['group_num'])
            for j in i['pipe_list']:
                p=Pipe(j['pipe_num'],j['pipediameter'],j['portType'],j['startpoint'],j['endpoint'],j['startvector'],j['endvector'])
                if j['constraint_list'] != None:
                    p.setconstraint(j['constraint_list'],j['waypoint'])
                g.pipe_list.append(p)
            self.group_manage.append(g)

class Group_list:
    def __init__(self, group_id):
        self.group_id = group_id
        self.pipe_list =[]
    # 生成所有可能的管路排列组合
    def generate_sequence(self):
        #pipes = [pipe for pipe in self.pipe_list]
        # 返回所有排列组合
        return list(permutations(self.pipe_list))
class Pipe:
    def __init__(self, pipe_id,r,portType,spoint, epoint,svector,evector):
        self.pipe_id = pipe_id
        self.r = r
        self.portType = portType
        self.spoint = spoint
        self.epoint = epoint
        self.svector = svector
        self.evector = evector
        self.constraint_list = []
        self.waypoint_list = []
        self.astarpath=None
    def setconstraint(self,constraints,waypoint_list):
        for i in constraints:
            rc = route_constraint(i['name'])
            rc.contralPoints = i['contralPoints']
            rc.middlePoints = i['middlePoints']
            rc.vector = i['vector']
            rc.extensionPoints = i['extensionPoints']
            self.constraint_list.append(rc)
        self.waypoint_list = waypoint_list
class route_constraint:
    def __init__(self, name):
        self.name = name
        self.contralPoints = []
        self.middlePoints = []
        self.vector = []
        self.extensionPoints = []
    
class components:
    def __init__(self, name):
        self.name = name
        self.component=[]
        self.kafcomponent=[]
        self.pipeSlist=[]
        self.generatrixs=[]
        self.casing=None
    def setdata(self,data):
        if data['kafcomponent']==None:
            self.kafcomponent=None
        else:
            pass
        if data['component']==None:
            self.component=None
        else:
            for i in data['component']:
                cp=component(i['name'])
                for j in i['boxlist']:
                    cp.boxlist.append(box(j['name'],j['centerPoint'],j['coordpoints'],j['csy']['data']))
                self.component.append(cp)
        if data['pipeSlist']==None:
            self.pipeSlist=None
        else:
            for i in data['pipeSlist']:
                self.pipeSlist.append(PipeS(i['p1'],i['p2'],i['d']))
        if data['generatrixs']==None:
            self.generatrixs=None
        else:
            for i in data['generatrixs']:
                self.generatrixs.append(Generatrix(i['x'],i['d']))
class Generatrix:
    def __init__(self,x,d):
        self.x=x
        self.d=d

#用两点定义线段
class PipeS:
    def __init__(self, p1, p2,r):
        self.p1 = np.array(p1)
        self.p2 = np.array(p2)
        self.r = r
        #self.length = math.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2+ (p2[2] - p1[2])**2)
        #self.center = ((p1[0] + p2[0])/2, (p1[1] + p2[1])/2, (p1[2] + p2[2])/2)

class component:
    def __init__(self, name):
        self.name = name
        self.boxlist=[]

class box:
    def __init__(self, name,centerPoint,coordpoints,csy):
        self.name = name
        self.centerPoint = centerPoint
        self.coordpoints = coordpoints
        self.csy = csy
        self.half_extents=None
        self.boxrange=None

class griddata:
    def __init__(self,routing_id,Grouptask,minpoints,maxpoints,centerpoint,resolution_rate):
        self.Grouptask = Grouptask
        self.routing_id=routing_id
        self.minpoints = minpoints
        self.maxpoints = maxpoints
        self.centerpoint = centerpoint
        self.resolution_rate = resolution_rate


class PSI:#计算两空间线段的最近距离及干涉情况
    def __init__(self,segment1,segment2, psi_threshold=0.001):
        self.psi_threshold = psi_threshold
        self.segment1 = segment1
        self.segment2 = segment2
        self.P0=self.segment1.p1
        self.P1=self.segment1.p2
        self.Q0=self.segment2.p1
        self.Q1=self.segment2.p2

        self.d1=self.segment1.d1
        self.d2=self.segment2.d1
        #R(s, t) = |P (s) − Q(t)|**2 = as**2 − 2bst + ct**2 + 2ds − 2et + f

        #self.a = (self.P1-self.P0).dot(self.P1-self.P0)
        #self.b = (self.P1-self.P0).dot(self.Q1-self.Q0)
        #self.c = (self.Q1-self.Q0).dot(self.Q1-self.Q0)
        #self.d = (self.P1-self.P0).dot(self.P0-self.Q0)
        #self.e = (self.Q1-self.Q0).dot(self.P0-self.Q0)
        #self.f = (self.P0-self.Q0).dot(self.P0-self.Q0)
        self.a=np.dot(self.P1-self.P0,self.P1-self.P0)
        self.b=np.dot(self.P1-self.P0,self.Q1-self.Q0)
        self.c=np.dot(self.Q1-self.Q0,self.Q1-self.Q0)
        self.d=np.dot(self.P1-self.P0,self.P0-self.Q0)
        self.e=np.dot(self.Q1-self.Q0,self.P0-self.Q0)
        self.f=np.dot(self.P0-self.Q0,self.P0-self.Q0)

    #公式法计算，存在误差
    def intersection(self):
        det=Decimal(self.a*self.c)-Decimal(self.b*self.b)
        if det>0:
            bte=(Decimal(self.b)*Decimal(self.e))
            ctd=(Decimal(self.c)*Decimal(self.d))
            if(bte<=ctd):
                if(self.e<=0):
                    s=(-self.d>=self.a and 1 or (-self.d>0 and Decimal(-self.d)/Decimal(self.a) or 0))
                    t=0
                elif(self.e<self.c):
                    s=0
                    t=(Decimal(self.e)/Decimal(self.c))
                else:
                    s=(Decimal(self.b)-Decimal(self.d)>Decimal(self.a) and 1 or (Decimal(self.b)-Decimal(self.d)>0 and (Decimal(self.b)-Decimal(self.d))/Decimal(self.a) or 0))
                    t=1
            else:
                s=Decimal(bte)-Decimal(ctd)
                if(s>=det):
                    if(Decimal(self.b)+Decimal(self.e)<=Decimal(0)):
                        s=(-self.d<=0 and 0 or (-self.d<self.a and Decimal(-self.d)/Decimal(self.a) or 1))
                        t=0
                    elif(Decimal(self.b)+Decimal(self.e)<Decimal(self.c)):
                        s=1
                        t=(Decimal(self.b)+Decimal(self.e))/Decimal(self.c)
                    else:
                        s=(Decimal(self.b)-Decimal(self.d)<=0 and 0 or (Decimal(self.b)-Decimal(self.d)<self.a and  (Decimal(self.b)-Decimal(self.d))/Decimal(self.a) or 1))
                        t=1
                else:
                    ate=Decimal(self.a)*Decimal(self.e)
                    btd=Decimal(self.b)*Decimal(self.d)
                    if(ate<=btd):
                        s=(-self.d<=0 and 0 or (-self.d>=self.a and 1 or Decimal(-self.d)/Decimal(self.a)))
                        t=0
                    else:
                        t=Decimal(ate)-Decimal(btd)
                        if(t>=det):
                            s=(Decimal(self.b)-Decimal(self.d)<=0 and 0 or (Decimal(self.b)-Decimal(self.d)>=Decimal(self.a) and 1 or (Decimal(self.b)-Decimal(self.d))/Decimal(self.a)))
                            t=1
                        else:
                            #s/=det
                            #t/=det
                            s=Decimal(s)/Decimal(det)
                            t=Decimal(t)/Decimal(det)
        else:
            if(self.e<=0):
                s=(-self.d<=0 and 0 or (-self.d>=self.a and 1 or Decimal(-self.d)/Decimal(self.a) ))
                t=0
            elif(self.e>=self.c):
                s=(Decimal(self.b)-Decimal(self.d)<=0 and 0 or (Decimal(self.b)-Decimal(self.d)>=self.a and 1 or (Decimal(self.b)-Decimal(self.d))/Decimal(self.a)))
                t=1

            else:
                s=0
                t=(Decimal(self.e)/Decimal(self.c))

        s=float(s)
        t=float(t)

        distance=np.linalg.norm(((1-s)*self.P0+s*self.P1)-((1-t)*self.Q0+t*self.Q1))
        if distance>(self.d1+self.d2):
            itf=0
        else:
            itf=1

        return s,t,distance,itf
    #稳定算法
    def robust_intersection(self):
        f00=self.d
        f10=self.d+self.a
        f01=self.d-self.b
        f11=self.d+self.a-self.b

        g00=-self.e
        g10=-self.e-self.b
        g01=-self.e+self.c
        g11=-self.e-self.b+self.c
      
        parameter=[0,0]
        if(self.a>0 and self.c>0):
            hatS=[0,0]
            #print(self.a,f00,f10)
            hatS[0]=self.GetClampedRoot(self.a,f00,f10)
            hatS[1]=self.GetClampedRoot(self.a,f01,f11)
            classify=[0,1]
            #print(hatS[0],hatS[1])
           # classify[0]=(hatS[0]<=0 and -1 or (hatS[0]>=1 and 1 or 0))
           # classify[1]=(hatS[1]<=0 and -1 or (hatS[1]>=1 and 1 or 0))
            
            for i in range(2):
                if(hatS[i]<=0):
                    classify[i]=-1
                elif(hatS[i]>=1):
                    classify[i]=+1
                else:
                    classify[i]=0


            if(classify[0]==-1 and classify[1]==-1):#s=0  0<=t<=1
                parameter[0]=0
                parameter[1]=self.GetClampedRoot(self.c,g00,g01)
            elif(classify[0]==1 and classify[1]==1):#s=1 0<=t<=1
                parameter[0]=1
                parameter[1]=self.GetClampedRoot(self.c,g10,g11)
            else:
                edge=[0,0]
                end=np.zeros((2,2))
                edge,end=self.ComputerIntersection(hatS,classify,self.b,f00,f10)
                parameter=self.ComputerMinimumParameters(edge,end,self.b,self.c,self.e,g00,g10,g01,g11)
        else:
            if(self.a>0):
               parameter[0]=self.GetClampedRoot(self.a,f00,f10)
               parameter[1]=0
            elif(self.c>0):
               parameter[0]=0
               parameter[1]=self.GetClampedRoot(self.c,g00,g01)
            else:
                parameter[0]=0
                parameter[1]=0
        
        closest=[0,0]
        closest[0]=(1-parameter[0])*self.P0+parameter[0]*self.P1
        closest[1]=(1-parameter[1])*self.Q0+parameter[1]*self.Q1
        diff=closest[0]-closest[1]
        #sqrDistance=(diff).dot(diff)
        #distance=np.sqrt(sqrDistance)
        distance=np.linalg.norm(diff)
        if distance>(self.d1+self.d2):
            itf=0
        else:
            itf=1
        return parameter[0],parameter[1],distance,itf

    def GetClampedRoot(self,real_sigma,real_h0,real_h1):
        root=0
        if(real_h0<0):
           
            if(real_h1>0):
                root=-real_h0/real_sigma
                if root>1:
                    root=0.5
            else:
                root=1
        else:
            root=0
        return root

    def ComputerIntersection(self,hatS,classify,b,f00,f10):
        zero=0
        half=0.5
        one=1
        edge=[0,0]
        end=np.zeros((2,2))
        if(classify[0]<0):
            edge[0]=0
            end[0][0]=zero
            end[0][1]=f00/b
            if(end[0][1]<zero or end[0][1]>one):
                end[0][1]=half
            
            if(classify[1]==0):
                edge[1]=3
                end[1][0]=hatS[1]
                end[1][1]=one
            else:#classify[1]>0
                edge[1]=1
                end[1][0]=one
                end[1][1]=f10/b
                if(end[1][1]<zero or end[1][1]>one):
                    end[1][1]=half
        elif classify[0]==0 :
            edge[0]=2
            end[0][0]=hatS[0]
            end[0][1]=zero
            if(classify[1]<0):
                edge[1]=0
                end[1][0]=zero
                end[1][1]=f00/b
                if(end[1][1]<zero or end[1][1]>one):
                    end[1][1]=half
            elif classify[1]==0:
                edge[1]=3
                end[1][0]=hatS[1]
                end[1][1]=one
            else:
                edge[1]=1
                end[1][0]=one
                end[1][1]=f10/b
                if(end[1][1]<zero or end[1][1]>one):
                    end[1][1]=half
        else:
            edge[0]=1
            end[0][0]=one
            end[0][1]=f10/b

            if(end[0][1]<zero or end[0][1]>one):
                end[0][1]=half
            if(classify[1]==0):
                edge[1]=3
                end[1][0]=hatS[1]
                end[1][1]=one
            else:  
                edge[1]=0
                end[1][0]=zero
                end[1][1]=f00/b
                if(end[1][1]<zero or end[1][1]>one):
                    end[1][1]=half
        return edge, end            

    def ComputerMinimumParameters(self,edge,end,b,c,e,g00,g10,g01,g11):
        parameter=[0,0]
        zero=0
        one=1
        delta=end[1][1]-end[0][1]
        h0=delta*(-b*end[0][0]+c*end[0][1]-e)
        if(h0>=zero):
            if(edge[0]==0):
                parameter[0]=zero
                parameter[1]=self.GetClampedRoot(c,g00,g01)
            elif(edge[0]==1):
                parameter[0]=one
                parameter[1]=self.GetClampedRoot(c,g10,g11)
            else:
                parameter[0]=end[0][0]
                parameter[1]=end[0][1]
        else:
            h1=delta*(-b*end[1][0]+c*end[1][1]-e)
            if(h1<=zero):
                if(edge[1]==0):
                    parameter[0]=zero
                    parameter[1]=self.GetClampedRoot(c,g00,g01)
                elif(edge[1]==1):
                    parameter[0]=one
                    parameter[1]=self.GetClampedRoot(c,g10,g11)
                else:
                    parameter[0]=end[1][0]
                    parameter[1]=end[1][1]
            else:
                z=min(max(h0/(h0-h1),zero),one)
                omz=one-z
                parameter[0]=omz*end[0][0]+z*end[1][0]
                parameter[1]=omz*end[0][1]+z*end[1][1]
        return parameter

class PPD:#评估两空间线段并行程度
    def __init__(self,segment1,segment2, psi_threshold=0.001):
        self.psi_threshold = psi_threshold
        self.segment1 = segment1
        self.segment2 = segment2
        self.P0=self.segment1.p1
        self.P1=self.segment1.p2
        self.Q0=self.segment2.p1
        self.Q1=self.segment2.p2

        self.d1=self.segment1.d1
        self.d2=self.segment2.d1

    def ParalleDifferenceDegree(self):
        v1=self.P1-self.P0
        v2=self.Q1-self.Q0
    #求点到直线投影点坐标
    def P_projected2segments(self):
        point=self.P0
        line=self.segment2

def random_point(): return (100*random.random(), 100*random.random(), 100*random.random())

def random_vector(): return (random.random(), random.random(), random.random())

def random_distance(): return random.random()

def convert_to_cyl(x,y,z):
    # 将点转换为圆柱坐标
    r = math.sqrt(y**2 + z**2)
    theta = math.atan2(z, y)
    z = x
    return (r, theta, z)

def convert_to_coord(r, theta, z):
    # 将圆柱坐标转换为笛卡尔坐标
    x = z
    y = r * math.cos(theta)
    z = r * math.sin(theta)
    return (x, y, z)

def Radialinterpolation(cmps, x, height):
    #插值函数，依据输入的x坐标，在cmps['generatrixs']中找到对应的y坐标并偏移给定高度
    
    x_max=cmps.generatrixs[-1].x
    x_min=cmps.generatrixs[0].x
    if x>=x_max :
        return x_max+height
    elif  x<=x_min:
        return x_min+height
    else:
        for i in range(len(cmps.generatrixs)-1):
            if cmps.generatrixs[i].x<=x and cmps.generatrixs[i+1].x>=x:
                x1=cmps.generatrixs[i].x
                x2=cmps.generatrixs[i+1].x
                y1=cmps.generatrixs[i].d
                y2=cmps.generatrixs[i+1].d
                y=(y2-y1)/(x2-x1)*(x-x1)+y1
                return y+height

class PBI:
    def pbi(self, obb, pipes):
        obb_vertices_coord = obb.coordpoints
        start_point_coord = np.array(pipes.p1)
        end_point_coord = np.array(pipes.p2)

        proj_axis = []  # Store all potential separating axes

        # Line segment direction as a separating axis
        line_direction = end_point_coord - start_point_coord
        line_direction = line_direction / np.linalg.norm(line_direction)
        proj_axis.append(line_direction)

        # Find OBB's three directions as separating axes
        for i in range(0, 9, 3):
            tmp_proj_axis = np.array([obb.csy.data[i], obb.csy.data[i + 1], obb.csy.data[i + 2]])
            proj_axis.append(tmp_proj_axis)

        # Compute cross products between line direction and OBB axes
        for i in range(1, 4):
            obb_cross_line_proj_axis = np.cross(line_direction, proj_axis[i])
            obb_cross_line_proj_axis = obb_cross_line_proj_axis / np.linalg.norm(obb_cross_line_proj_axis)
            proj_axis.append(obb_cross_line_proj_axis)

        # Store computed values
        min1, min2, max1, max2 = 0.0, 0.0, 0.0, 0.0

        # Project onto each separating axis
        for axis in proj_axis:
            min1 = np.dot(start_point_coord, axis)
            max1 = min1
            dot1 = np.dot(end_point_coord, axis)

            if dot1 < min1: min1 = dot1
            if dot1 > max1: max1 = dot1

            min2 = np.dot(obb_vertices_coord[0], axis)
            max2 = min2

            for vertex in obb_vertices_coord:
                dot2 = np.dot(vertex, axis)

                if dot2 < min2: min2 = dot2
                if dot2 > max2: max2 = dot2

            # Check if projections overlap
            if max1 < min2 or max2 < min1:
                return False  # No collision on this axis

        return True  # Collision detected

class JudgeResult:
    def judge1(self, ps, cmps):
        ps2=cmps.pipeSlist
        cp=cmps.component
        for i in ps:
            jud=0
            psi=PSI(i,cmps.casing)
            s, t, distance, itf = psi.robust_intersection()
            if itf==1:
                jud=1
                p1=np.array([i.p1[0],np.sqrt(i.p1[1]**2+i.p1[2]**2)])
                p2=np.array([i.p2[0],np.sqrt(i.p2[1]**2+i.p2[2]**2)])
                sortedPoints=[p1,p2]
                if 0<=s and s<=1:
                    closest=(1-s)*p1+s*p2
                    p3=np.array([closest[0],np.sqrt(closest[1]**2+closest[2]**2)])
                    sortedPoints.append(p3)
                sortedPoints.sort(key=lambda x: x[0])
                for m in sortedPoints:
                    masws = Radialinterpolation(cmps,m[0],i.r)
                    if masws<m[1]:
                        continue
                    elif masws>=m[1]:
                        return 1
            for j in ps2:
                psi=PSI(i,j)
                s, t, distance, itf = psi.robust_intersection()
                if itf==1:
                    return 1
            pbi = PBI()
            for m in cp:
                if m.name!='pipe':
                    for n in m.boxlist:
                        if pbi.pbi(n, i):
                            return 1            
        return 0
    
    def judge2(self, ps, cmps):
        jud1=0
        ps2=cmps.pipeSlist
        cp=cmps.component
        for i in ps:
            jud=0
            psi=PSI(i,cmps.casing)
            s, t, distance, itf = psi.robust_intersection()
            if itf==1:
                jud=1
                p1=np.array([i.p1[0],np.sqrt(i.p1[1]**2+i.p1[2]**2)])
                p2=np.array([i.p2[0],np.sqrt(i.p2[1]**2+i.p2[2]**2)])
                sortedPoints=[p1,p2]
                if 0<=s and s<=1:
                    closest=(1-s)*p1+s*p2
                    p3=np.array([closest[0],np.sqrt(closest[1]**2+closest[2]**2)])
                    sortedPoints.append(p3)
                sortedPoints.sort(key=lambda x: x[0])
                for m in sortedPoints:
                    masws = Radialinterpolation(cmps,m[0],i.r)
                    if masws<m[1]:
                        continue
                    elif masws>=m[1]:
                        jud=5
                        break
            else :
                pass
            if jud!=0:
                jud1+=jud
                continue
            jud=0
            for j in ps2:
                psi=PSI(i,j)
                s, t, distance, itf = psi.robust_intersection()
                if itf==1:
                    jud=1
                    break
            if jud!=0:  
                jud1+=jud
                continue
            jud=0
            pbi = PBI()
            for m in cp:
                if m.name!='pipe':
                    for n in m.boxlist:
                        if pbi.pbi(n, i):
                            jud=1
                            break
            if jud!=0:
                jud1+=jud
                continue
        return jud1